using UnityEngine;
using System.Collections;
using System;
using System.Linq;
using System.Collections.Generic;

public class AStar : MonoBehaviour {
	
	//distance between nodes
	public float nodeSpacing = 8f;
	
	public float maxDistance = 10f;
	public int arraySize = 100;
	public bool debugGraph = false;
	public float heightAboveGround = 1f;
	public float allowedHeight = 30f;
	public float graphCutHeight = 45f;
	public float allowedNodeHeightDifference = 2f;
	
	//privates
	private Vector3 graphPosition;
	
	node[,] nodes;
	
	List<node> closedset = new List<node>();
	List<node> openset = new List<node>();
	
	
	List<node> path = new List<node>();
	
	public AStar(Vector3 graphPosition){
		graphPosition = graphPosition;
		nodes = new node[arraySize,arraySize];
		generateGraph();
	}
	
	//Class for node
	public class node{	
		public Vector3 pos;
		public bool inClosed;
		public bool inOpen;
		public List<node> connections;
		public node parent;
		public float g;
		public float f;
		
		public void reset(){
			inOpen   = false;
			inClosed = false;
			parent   = null;
			g 		 = 9999f;
			f 		 = 9999f;
			
		}

	}
	
	public List<node> GetPath(Vector3 start,Vector3 goal){
		Astar(start,goal);
		return path;
	}
	
	public List<node> GetPath(Vector3 start){

		while(path.Count==0){
			Vector3 goal = new Vector3(UnityEngine.Random.Range(000.0F, 200.0F),0,UnityEngine.Random.Range(000.0F, 200.0F));
			if((start.y-goal.y)>-allowedHeight){
				goal = findY(goal);	
				Astar(start,goal);
			}
		}
		Debug.Log("pathcount: "+path.Count);
		return path;
	}
	
	void Start () {
		graphPosition = transform.position;
		nodes = new node[arraySize,arraySize];
		generateGraph();
//		Astar(new Vector3(0,2,0),new Vector3(20,2,200));
	}

	// Determining terrain height at the provided position.
	// Discards the positions current Y and replaces with found position.
	// if no position found then return zero vector.
	Vector3 findY(Vector3 pos)
	{
		Ray ray = new Ray(new Vector3(pos.x,1000,pos.z),Vector3.down);
		RaycastHit hit = new RaycastHit();
		
		if(Physics.Raycast(ray,out hit,5000f))	{
			pos.y = hit.point.y+heightAboveGround; 
			return pos;			
		}
		return Vector3.zero;
	}
	
	void OnDrawGizmos()	{

		if(debugGraph && nodes != null){
			for(int i = 0;i<arraySize;i++){
				for(int j = 0;j<arraySize;j++){
					if(nodes[i,j] != null){
						Gizmos.color = Color.green;
						Gizmos.DrawSphere(nodes[i,j].pos,1);
						for(int k = 0; k<nodes[i,j].connections.Count;k++){
							Debug.DrawLine(nodes[i,j].pos,nodes[i,j].connections[k].pos);
						}
					}
				}
			}
			Debug.Log(path.Count);
			for(int i = 0;i<path.Count;i++){
				node n = path[i];
				if(n.parent != null){
					Gizmos.color = Color.red;
					Gizmos.DrawLine(path[i].pos,path[i].parent.pos);
				}
			}
		}
	}
	
	void Astar(Vector3 start, Vector3 goal)
	{
		
		if(nodes.Length<10){
			generateGraph();
		}
		
		resetGraph();
		path.Clear();
		node n = findClosestNode(start);
		//Debug.Log("Start:" + n.pos);
		n.g = 0;
		n.f = 0;
		n.parent = null;	
		n.inOpen = true;
		openset.Add(n);
		node target = findClosestNode(goal);
		//Debug.Log("Target:" + target.pos);
		//guard so it wont get stuck
		int b = 0;
		
		while(openset.Count>0&&b<3500){
			b++;
			node current = findLowestScore();
			
			if(heuristic(current.pos,target.pos)<15){
				//Debug.Log("Success");
				reconstructPath(current);
				break;
			}else{
				addNeighbours(current,target);
			}
			
			openset.Remove(current);
			closedset.Add(current);
			
			current.inOpen = false;
			current.inClosed = true;
			
			/*
			//Debug.Log(current.connections.Count);
			//for each neighbor node in current
			for(int i = 0;i<current.connections.Count;i++){;
				node neighbour = current.connections[i];
				float tentative_g = current.g+1;
				if(neighbour.inClosed){
					//Debug.Log("found in closed");
					if(tentative_g>=neighbour.g){
						continue;
					}
				}
				if(!neighbour.inOpen
				||	tentative_g < neighbour.g){
					if(!neighbour.inOpen){
						openset.Add(neighbour);
						neighbour.inOpen = true;
						Debug.Log("found in open");
					}
				}
			}*/
			
		}
	
	}
	
	node findClosestNode(Vector3 pos){
		
		float diff = 999f;
		node closest = null;
		
		for(int i = 0;i<arraySize;i++){
			for(int j = 0;j<arraySize;j++){
				if(nodes[i,j] != null){
					if((nodes[i,j].pos-pos).magnitude<diff){
						closest = nodes[i,j];
						diff = (nodes[i,j].pos-pos).magnitude;
					}
				}
			}
		}
		return closest;
	}
	
	void reconstructPath(node end)
	{
		path.Add(end);
		
		bool parent = true;
		while(parent)
		{
			if(end != null){
				if(end.parent != null){
					path.Add(end.parent);
					end=end.parent;
				}else{
					parent = false;
				}
			}
		}

	}
	
	node findLowestScore()
	{
		float f = 99999f;
		node lowest = null;
		for(int i = 0;i<openset.Count;i++){
			if(openset[i].f<f){
				f = openset[i].f;
				lowest = openset[i];
			}
		}
	//	Debug.Log(openset.Count + "Lowest Score:" + lowest.f);
		return lowest;


	}
	
	void addNeighbours(node n,node goal)
	{
		for(int i = 0;i<n.connections.Count;i++)
		{
			if(!n.connections[i].inClosed && !n.connections[i].inOpen){				
				n.connections[i].g=n.g+nodeSpacing;
				n.connections[i].f=n.connections[i].g+heuristic(n.connections[i].pos,goal.pos);
				n.connections[i].parent = n;
				n.connections[i].inOpen = true;
				openset.Add(n.connections[i]);
			}
		}
	}
	
	
	float heuristic(Vector3 start, Vector3 goal)
	{
		return (goal-start).magnitude;
	}
	
	void resetGraph(){
		if(nodes != null){
			for(int i = 0;i<arraySize;i++){
				for(int j = 0;j<arraySize;j++){
					if(nodes[i,j] != null)
						nodes[i,j].reset();
				}
			}
		}
	}
	public void recreateGraph(){
		generateGraph();
	}
	
	void generateGraph(){
		Debug.Log("generating graph");
		Vector3 increase = new Vector3(nodeSpacing,0,nodeSpacing);
		Vector3 start = graphPosition+(increase*-arraySize/2);
		
		Vector3 increaseX = new Vector3(nodeSpacing,0,0);
		Vector3 increaseZ = new Vector3(0,0,nodeSpacing);
		
		for(int i = 0;i<arraySize;i++){
			for(int j = 0;j<arraySize;j++){
				nodes[i,j] = new node();
				nodes[i,j].connections = new List<node>();
				nodes[i,j].pos = start+i*increaseX+j*increaseZ;
				nodes[i,j].pos.y = findY(nodes[i,j].pos).y;
				nodes[i,j].parent = null;
				if(nodes[i,j].pos.y<graphCutHeight){
					nodes[i,j]=null;
				}
			}
		}
		Debug.Log("generating connections");
		for(int i = 0;i<arraySize;i++){
			for(int j = 0;j<arraySize;j++){
				
				if(i!=0
				&&i!=arraySize-1
				&&j!=0
				&&j!=arraySize-1){
					for(int k = -1;k<=1;k++){
						for(int l = -1;l<=1;l++){
							if(nodes[i,j] != null && nodes[i-k,j-l] != null){
								if(!Physics.Linecast(nodes[i,j].pos,nodes[i-k,j-l].pos)&& (nodes[i,j].pos-nodes[i-k,j-l].pos).magnitude<maxDistance){
									if(Mathf.Abs(nodes[i,j].pos.y-nodes[i-k,j-l].pos.y)<allowedNodeHeightDifference){
										nodes[i,j].connections.Add(nodes[i-k,j-l]);
									}
								}
							}
						}
					}
				}
			}			
		}
				
	}
}
